#!/usr/bin/env python
#coding=utf-8

import rospy
# import sys, tf, actionlib, moveit_commander
from gazebo_msgs.srv import *
from geometry_msgs.msg import *


if __name__ == "__main__":
    rospy.init_node('reset_racecar')

    rospy.wait_for_service('/gazebo/set_model_state')
    set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # 设置模型位置服务

    objstate = SetModelStateRequest()
    # 重置小车
    print("-"*20 + "Reset the car" + "-"*20)
    objstate = SetModelStateRequest()
    objstate.model_state.model_name = "racecar"
    objstate.model_state.pose.position.x = -0.3
    objstate.model_state.pose.position.y = -0.3
    objstate.model_state.pose.position.z = 0
    objstate.model_state.pose.orientation.w = 0.707
    objstate.model_state.pose.orientation.x = 0
    objstate.model_state.pose.orientation.y = 0
    objstate.model_state.pose.orientation.z = 0.707
    objstate.model_state.twist.linear.x = 0
    objstate.model_state.twist.linear.y = 0
    objstate.model_state.twist.linear.z = 0
    objstate.model_state.twist.angular.x = 0
    objstate.model_state.twist.angular.y = 0
    objstate.model_state.twist.angular.z = 0
    objstate.model_state.reference_frame = "world"
    result = set_state_service(objstate)